clc;
clear;

%% main主脚本
tic;
startTime = cputime;
pde = process_pde_NS_t(5);
vector_h = [1/4 1/8 1/16 1/32];
basis_type1 = "P2";
basis_type2 = "P1";

u_Linf = zeros(size(vector_h,2),1);
u_L2 = zeros(size(vector_h,2),1);
u_H1 = zeros(size(vector_h,2),1);
p_Linf = zeros(size(vector_h,2),1);
p_L2 = zeros(size(vector_h,2),1);
p_H1 = zeros(size(vector_h,2),1);

for i = 1:size(vector_h,2)
    h = vector_h(i);
    dt = h^2;
    Nt = ceil((pde.domain(6) - pde.domain(5))/dt);

    mesh = process_mesh(pde, h);
    fs1 = process_fespace(mesh, basis_type1);
    fs2 = process_fespace(mesh, basis_type2);

    % process_showmesh(mesh, fs1);
    % process_showmesh(mesh, fs2);

    A11 = term_U_V(mesh, fs1);
    A13 = pde.nu*term_grad_U_grad_V(mesh, fs1);

    A21 = term_P_Q(mesh, fs2);

    A = 3/(2*dt)*A11 + A13;

    U0 = [pde.u1(fs1.node(:,1),fs1.node(:,2),0);
        pde.u2(fs1.node(:,1),fs1.node(:,2),0)];
    U1 = [pde.u1(fs1.node(:,1),fs1.node(:,2),dt);
        pde.u2(fs1.node(:,1),fs1.node(:,2),dt)];
    P0 = pde.p(fs2.node(:,1),fs2.node(:,2),0);
    s = sparse(fs2.Nb,1);

    for m = 1:Nt-1
        t = (m+1)*dt;

        Uext = 2*U1-U0;
        AN = term_Uold_grad_U_V(mesh, fs1, Uext);
        Au = A + AN;
        b1 = term_F_V_t(mesh, fs1, pde.f1, pde.f2, t);
        b2 = pde.nu*term_Pold_div_V(mesh, fs1, fs2, s);
        b3 = term_Pold_div_V(mesh, fs1, fs2, P0);
        bu = b1 + b2 + b3 + 2/dt*A11*U1 - 1/(2*dt)*A11*U0;
        [Au, bu] = process_boundary_Uzawa(Au, bu, mesh, fs1, pde, t);
        Uhat = Au\bu;

        Apho = term_grad_P_grad_Q(mesh, fs2) + 1e-6*A21;
        bpho = term_div_Uold_Q(mesh, fs1, fs2, Uhat);
        pho = Apho\bpho;

        As = A21;
        bs = A21*s - bpho;
        s = As\bs;

        Auu = A11;
        buu = term_grad_Pold_V(mesh, fs1, fs2, pho);
        buu = A11*Uhat + buu;
        [Auu, buu] = process_boundary_Uzawa(Auu, buu, mesh, fs1, pde, t);
        U2 = Auu\buu;

        P1 = P0 - 3/(2*dt)*pho + pde.nu*s;

        U0 = U1;
        U1 = U2;
        P0 = P1;
    end
    SOL = [U2; P1];

    % process_plot(mesh, fs1, fs2, SOL);
    % process_field(fs1, fs2, SOL);

    [u_Linf(i,1), p_Linf(i,1)] = process_error_NS_t(mesh, fs1, fs2, pde, SOL, "Linf");
    [u_L2(i,1), p_L2(i,1)] = process_error_NS_t(mesh, fs1, fs2, pde, SOL, "L2");
    [u_H1(i,1), p_H1(i,1)] = process_error_NS_t(mesh, fs1, fs2, pde, SOL, "H1");

    fprintf("已完成\t%d/%d\n", i, size(vector_h,2));
end

process_rate(u_Linf, vector_h);
process_rate(u_L2, vector_h);
process_rate(u_H1, vector_h);
process_rate(p_Linf, vector_h);
process_rate(p_L2, vector_h);
process_rate(p_H1, vector_h);

TOCtime = toc;
CPUtime = cputime - startTime;
disp(["TOC time(s)", num2str(TOCtime)]);
disp(["CPU time(s)", num2str(CPUtime)]);

%% process_boundary_Uzawa
function [A, b] = process_boundary_Uzawa(A, b, mesh, fespace, pde, t)
Nb = fespace.Nb;
% Dirichlet
for k = 1:size(pde.bdary,2)
    if 0 == pde.bdary(1,k)
        bdn1 = bdary_node(mesh, fespace, k);
        A(bdn1,:) = 0;
        A(sub2ind(size(A), bdn1, bdn1)) = 1;
        b(bdn1) = pde.u1(fespace.node(bdn1,1),fespace.node(bdn1,2),t);
        bdn2 = bdn1 + Nb;
        A(bdn2,:) = 0;
        A(sub2ind(size(A), bdn2, bdn2)) = 1;
        b(bdn2) = pde.u2(fespace.node(bdn1,1),fespace.node(bdn1,2),t);
    end
end
end